
#ifndef __PID_H_
#define __PID_H_
#include <string>
#include "rclcpp/rclcpp.hpp"
#include <rclcpp_lifecycle/lifecycle_node.hpp> 
struct pid_param
{
    double Kp;
    double Ki;
    double Kd;
    double integral_limit;     // 积分饱和限
    double integral_threshold; // 积分分离阈值
    double upper_limit;        // 输出上限
    double lower_limit;        // 输出下限
    double period;             // 计算周期
    bool inverted;             // 输出反向
    pid_param()
    {
        Kp = 0.0;
        Ki = 0.0;
        Kd = 0.0;
        integral_limit = 0.0;
        integral_threshold = 0.0;
        upper_limit = 0.0;
        lower_limit = 0.0;
        period = 0.0;
        inverted = false;
    }
};

class PID
{
public:
    pid_param pid_param_; // 低速pid参数
    double err_;          // 当前误差
    double err1_;         // 上一次记录的误差
    double err2_;         // 上上次记录的误差
    double err_sum_;      // 误差累计值
    double output_;       // 控制量输出
    double output_last_;  // 控制量输出
    double p_term_;       // 比例计算量
    double i_term_;       // 积分量
    double d_term_;       // 微分量
    std::string m_name_;  //
    std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
public:
    // Default constructor
    PID(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node);
    // Generate  output
    double incrementalPID(double input,    // 输入反馈
                          double setpoint, // 设定值
                          bool halt,       // 暂停计算
                          bool reset);     // 复位所有值

    double positionPID(double input,
                       double setpoint,
                       bool halt,
                       bool reset);

    void clearCtrl();
};

#endif